

#include "board_timer.h"


volatile uint64_t system_time=0;
//static control_pid_est_t *local_board;
static sensor_raw_data_t *local_board;
static sensor_t *local_gyro;

/**
  * @brief  Init GPIO and TIMER2 channel like PWM.
  * @param  None
  * @retval None
  */
void timer_init(void)
{
  /* Init system time.*/
  system_time_interrupt_init();

  TIMER1_period_interupt_init(PERIOD_OF_INTERUPT);
  /* Init board PWM section.*/
#if TIMER1_PWM
  /* PWM on TIMER1. */
  TIMER1_PWM_channel_init(1,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
//  TIMER1_PWM_channel_init(2,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);//UART1 TX
//  TIMER1_PWM_channel_init(3,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);//UART1 RX
  TIMER1_PWM_channel_init(4,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);

  /* Start TIMER1. */
  TIM_Cmd(TIM1, ENABLE);
  TIM_CtrlPWMOutputs(TIM1, ENABLE);
#endif

#if TIMER2_PWM
  /* PWM on TIMER2. */
  TIMER2_PWM_channel_init(1,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER2_PWM_channel_init(2,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER2_PWM_channel_init(3,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER2_PWM_channel_init(4,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  /* Start TIMER2. */
  TIM_Cmd(TIM2, ENABLE);
#endif

#if TIMER3_PWM
  TIMER3_PWM_channel_init(1,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER3_PWM_channel_init(2,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
// PB0
  TIMER3_PWM_channel_init(3,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
// PB1
//  TIMER3_PWM_channel_init(4,PWM_PERIOD_TIMER2,PWM_DUTY_INITIAL_TIMER2); // Pin connected to led.
  /* Start TIMER4. */
  TIM_Cmd(TIM3, ENABLE);
#endif

  //TIMER3_CAPTURE_channel_init(2, TIM_ICSelection_DirectTI, TIM_ICPolarity_Rising, TIM_ICPSC_DIV1,0x00);

#if TIMER4_PWM
  TIMER4_PWM_channel_init(1,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER4_PWM_channel_init(2,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER4_PWM_channel_init(3,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  TIMER4_PWM_channel_init(4,PWM_PERIOD_DIGITAL,PWM_DUTY_INITIAL);
  /* Start TIMER4. */
  TIM_Cmd(TIM4, ENABLE);
#elif TIMER4_CAPTURE
  TIMER4_CAPTURE_channel_init(2, TIM_ICSelection_DirectTI, TIM_ICPolarity_Rising, TIM_ICPSC_DIV1,0x0F);
#endif


/* End of inboard PWM section. */






  /* It looks like it is neccessary only for TIMER1. */
  // TIM_CtrlPWMOutputs(TIM2, ENABLE);
}



/*****TIMER1*****/
/****************/
/**
  * @brief  Init GPIO and TIMER1 channel like PWM.
  * @param  ch_number: Specifies the TIMER1 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  pwm_duty: PWM duty cycle value (microSec).
  * @retval None
  */
void TIMER1_PWM_channel_init(
                               uint16_t ch_number, //1,2,3,4
                               uint16_t pwm_period,
                               uint16_t pwm_duty    //microSec
                            )
{
  GPIO_InitTypeDef GPIO_InitStructure;
  TIM_OCInitTypeDef TIM_OCInitStructure;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
     /* Connect clock to port A */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
  GPIO_InitStructure.GPIO_Mode        = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed       = GPIO_Speed_50MHz;
  TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OutputNState= TIM_OutputNState_Disable;
  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
  TIM_OCInitStructure.TIM_OCNIdleState= TIM_OCNIdleState_Reset;
  /* Duty cycle start value */
  TIM_OCInitStructure.TIM_Pulse       = pwm_duty;
  TIM_OCInitStructure.TIM_OCPolarity  = TIM_OCPolarity_High;

  /* Init Timer1 like PWM. */
  RCC_APB2PeriphClockCmd (RCC_APB2Periph_TIM1, ENABLE);
  // Time Base configuration
  TIM_TimeBaseStructure.TIM_Period        = pwm_period; //PWM period
  TIM_TimeBaseStructure.TIM_Prescaler     = 72;         // Ftimer=fsys/Prescaler,for Prescaler=72 ,Ftimer=1MHz
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

  if(ch_number == 1)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;  //PA8 -> TIM1_CH1
    TIM_OC1Init(TIM1, &TIM_OCInitStructure);
  }
  else if(ch_number == 2)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;  //PA9 -> TIM1_CH2
    TIM_OC2Init(TIM1, &TIM_OCInitStructure);
  }
  else if(ch_number == 3)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;  //PA10 -> TIM1_CH3
    TIM_OC3Init(TIM1, &TIM_OCInitStructure);
  }
  else if(ch_number == 4)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;  //PA11 -> TIM1_CH4
    TIM_OC4Init(TIM1, &TIM_OCInitStructure);
  }
  else
  {
    while(1){};
  }
  GPIO_Init(GPIOA, &GPIO_InitStructure);
}


/**
  * @brief  Set PWM duty for ch1 of TIMER1.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer1_PWM_duty_CH1(uint16_t duty /*microSec*/)
{
  TIM_SetCompare1(TIM1, duty);
}
/**
  * @brief  Set PWM duty for ch2 of TIMER1.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer1_PWM_duty_CH2(uint16_t duty /*microSec*/)
{
  TIM_SetCompare2(TIM1, duty);
}
/**
  * @brief  Set PWM duty for ch3 of TIMER1.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer1_PWM_duty_CH3(uint16_t duty /*microSec*/)
{
  TIM_SetCompare3(TIM1, duty);
}
/**
  * @brief  Set PWM duty for ch4 of TIMER1.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer1_PWM_duty_CH4(uint16_t duty /*microSec*/)
{
  TIM_SetCompare4(TIM1, duty);
}

/***** END TIMER1*****/


/*****TIMER2*****/
/****************/
/**
  * @brief  Init GPIO and TIMER2 channel like PWM.
  * @param  ch_number: Specifies the TIMER2 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  pwm_duty: PWM duty cycle value (microSec).
  * @retval None
  */
void TIMER2_PWM_channel_init(
                               uint16_t ch_number, //1,2,3,4
                               uint16_t pwm_period,
                               uint16_t pwm_duty    //microSec
                            )
{
  GPIO_InitTypeDef GPIO_InitStructure;
  TIM_OCInitTypeDef TIM_OCInitStructure;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
     /* Connect clock to port A */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
  GPIO_InitStructure.GPIO_Mode        = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed       = GPIO_Speed_50MHz;
  TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  /* Duty cycle start value */
  TIM_OCInitStructure.TIM_Pulse       = pwm_duty;
  TIM_OCInitStructure.TIM_OCPolarity  = TIM_OCPolarity_High;

  /* Init Timer2 like PWM. */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  // Time Base configuration
  TIM_TimeBaseStructure.TIM_Period        = pwm_period; //PWM period
  TIM_TimeBaseStructure.TIM_Prescaler     = 72;         // Ftimer=fsys/Prescaler,for Prescaler=72 ,Ftimer=1MHz
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);

  if(ch_number == 1)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;  //PA0 -> TIM2_CH1
    TIM_OC1Init(TIM2, &TIM_OCInitStructure);
  }
  else if(ch_number == 2)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;  //PA1 -> TIM2_CH2
    TIM_OC2Init(TIM2, &TIM_OCInitStructure);
  }
  else if(ch_number == 3)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;  //PA2 -> TIM2_CH3
    TIM_OC3Init(TIM2, &TIM_OCInitStructure);
  }
  else if(ch_number == 4)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;  //PA3 -> TIM2_CH4
    TIM_OC4Init(TIM2, &TIM_OCInitStructure);
  }
  else
  {
    while(1){};
  }
  GPIO_Init(GPIOA, &GPIO_InitStructure);
}


/**
  * @brief  Set PWM duty for ch1 of TIMER2.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer2_PWM_duty_CH1(uint16_t duty /*microSec*/)
{
  TIM_SetCompare1(TIM2, duty);
}
/**
  * @brief  Set PWM duty for ch2 of TIMER2.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer2_PWM_duty_CH2(uint16_t duty /*microSec*/)
{
  TIM_SetCompare2(TIM2, duty);
}
/**
  * @brief  Set PWM duty for ch3 of TIMER2.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer2_PWM_duty_CH3(uint16_t duty /*microSec*/)
{
  TIM_SetCompare3(TIM2, duty);
}
/**
  * @brief  Set PWM duty for ch4 of TIMER2.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer2_PWM_duty_CH4(uint16_t duty /*microSec*/)
{
  TIM_SetCompare4(TIM2, duty);
}

/***** END TIMER2*****/



/****************/
/*****TIMER3*****/
/****************/
/**
  * @brief  Init GPIO and TIMER3 channel like PWM.
  * @param  ch_number: Specifies the TIMER3 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  pwm_duty: PWM duty cycle value (microSec).
  * @retval None
  */
void TIMER3_PWM_channel_init(
                               uint16_t ch_number, //1,2,3,4
                               uint16_t pwm_period,
                               uint16_t pwm_duty    //microSec
                            )
{
  GPIO_InitTypeDef GPIO_InitStructure;
  TIM_OCInitTypeDef TIM_OCInitStructure;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  /* Connect clock to port A for ch 1(PA6) and ch2(PA7). */
  if((ch_number==1)||(ch_number==2))
  {
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    GPIO_InitStructure.GPIO_Mode        = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed       = GPIO_Speed_50MHz;
    TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  }
  else if((ch_number==3)||(ch_number==4))
  {
    /* Connect clock to port B for ch 3(PB0) and ch4(PB1). */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    GPIO_InitStructure.GPIO_Mode        = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Speed       = GPIO_Speed_50MHz;
    TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  }
  else
  {
    while(1){};
  }
  /* Duty cycle start value */
  TIM_OCInitStructure.TIM_Pulse       = pwm_duty;
  TIM_OCInitStructure.TIM_OCPolarity  = TIM_OCPolarity_High;

  /* Init Timer3 like PWM. */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
  // Time Base configuration
  TIM_TimeBaseStructure.TIM_Period        = pwm_period; //PWM period
  TIM_TimeBaseStructure.TIM_Prescaler     = 72;         // Ftimer=fsys/Prescaler,for Prescaler=72 ,Ftimer=1MHz
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

  if(ch_number == 1)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;  //PA6 -> TIM3_CH1
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    TIM_OC1Init(TIM3, &TIM_OCInitStructure);
  }
  else if(ch_number == 2)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;  //PA7 -> TIM3_CH2
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    TIM_OC2Init(TIM3, &TIM_OCInitStructure);
  }
  else if(ch_number == 3)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;  //PB0 -> TIM3_CH3
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    TIM_OC3Init(TIM3, &TIM_OCInitStructure);
  }
  else if(ch_number == 4)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;  //PB1 -> TIM3_CH4
    GPIO_Init(GPIOA, &GPIO_InitStructure);
    TIM_OC4Init(TIM3, &TIM_OCInitStructure);
  }
  else
  {
    while(1){};
  }
}

/**
  * @brief  Set PWM duty for ch1 of TIMER3.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer3_PWM_duty_CH1(uint16_t duty /*microSec*/)
{
  TIM_SetCompare1(TIM3, duty);
}
/**
  * @brief  Set PWM duty for ch2 of TIMER3.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer3_PWM_duty_CH2(uint16_t duty /*microSec*/)
{
  TIM_SetCompare2(TIM3, duty);
}
/**
  * @brief  Set PWM duty for ch3 of TIMER3.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer3_PWM_duty_CH3(uint16_t duty /*microSec*/)
{
  TIM_SetCompare3(TIM3, duty);
}
/**
  * @brief  Set PWM duty for ch4 of TIMER3.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer3_PWM_duty_CH4(uint16_t duty /*microSec*/)
{
  TIM_SetCompare4(TIM3, duty);
}

/*****END TIMER3*****/


/****************/
/*****TIMER4*****/
/****************/
/****************/
/**
  * @brief  Init GPIO and TIMER4 channel like PWM.
  * @param  ch_number: Specifies the TIMER3 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  pwm_duty: PWM duty cycle value (microSec).
  * @retval None
  */
void TIMER4_PWM_channel_init(
                               uint16_t ch_number, //1,2,3,4
                               uint16_t pwm_period,
                               uint16_t pwm_duty    //microSec
                            )
{
  GPIO_InitTypeDef GPIO_InitStructure;
  TIM_OCInitTypeDef TIM_OCInitStructure;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  /* Connect clock to port B . */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
  GPIO_InitStructure.GPIO_Mode        = GPIO_Mode_AF_PP;
  GPIO_InitStructure.GPIO_Speed       = GPIO_Speed_50MHz;
  TIM_OCInitStructure.TIM_OCMode      = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;

  /* Duty cycle start value */
  TIM_OCInitStructure.TIM_Pulse       = pwm_duty;
  TIM_OCInitStructure.TIM_OCPolarity  = TIM_OCPolarity_High;

  /* Init Timer4 like PWM. */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
  // Time Base configuration
  TIM_TimeBaseStructure.TIM_Period        = pwm_period; //PWM period
  TIM_TimeBaseStructure.TIM_Prescaler     = 72;         // Ftimer=fsys/Prescaler,for Prescaler=72 ,Ftimer=1MHz
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);

  if(ch_number == 1)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;  //PB6 -> TIM4_CH1
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    TIM_OC1Init(TIM4, &TIM_OCInitStructure);
  }
  else if(ch_number == 2)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;  //PB7 -> TIM4_CH2
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    TIM_OC2Init(TIM4, &TIM_OCInitStructure);
  }
  else if(ch_number == 3)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;  //PB8 -> TIM4_CH3
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    TIM_OC3Init(TIM4, &TIM_OCInitStructure);
  }
  else if(ch_number == 4)
  {
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;  //PB9 -> TIM4_CH4
    GPIO_Init(GPIOB, &GPIO_InitStructure);
    TIM_OC4Init(TIM4, &TIM_OCInitStructure);
  }
  else
  {
    while(1){};
  }
}

/**
  * @brief  Set PWM duty for ch1 of TIMER4.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer4_PWM_duty_CH1(uint16_t duty /*microSec*/)
{
  TIM_SetCompare1(TIM4, duty);
}
/**
  * @brief  Set PWM duty for ch2 of TIMER4.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer4_PWM_duty_CH2(uint16_t duty /*microSec*/)
{
  TIM_SetCompare2(TIM4, duty);
}
/**
  * @brief  Set PWM duty for ch3 of TIMER4.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer4_PWM_duty_CH3(uint16_t duty /*microSec*/)
{
  TIM_SetCompare3(TIM4, duty);
}
/**
  * @brief  Set PWM duty for ch4 of TIMER4.
  * @param  duty: Duty cycle value  in microSecond.
  * @retval None
  */
void timer4_PWM_duty_CH4(uint16_t duty /*microSec*/)
{
  TIM_SetCompare4(TIM4, duty);
}

/*****END TIMER4*****/


/********************** SYSTEM TIMER ***********************************/
/**
  * @brief  Initialisation of system timer interrupt for each milliSecond.
  * @param  None
  * @retval None
  */
void system_time_interrupt_init(void)
{
  /* Init interrupt eatch milliSecond. */
  SysTick_Config(SystemCoreClock / 1000);
  /* Set SysTick interrupt preemption priority. */
   NVIC_SetPriority(
                        SysTick_IRQn,
                        NVIC_EncodePriority(
                                                NVIC_GetPriorityGrouping(),
                                                SYSTEM_TICK_PRIORITY_GROUP,
                                                SYSTEM_TICK_SUB_PRIORITY_GROUP
                                            )
                   );
}

void SysTick_Handler(void)
{
   system_time++;
}
/**
  * @brief  Read current system time from start of board in milliSecond.
  * @param  None.
  * @retval System time in milliSecond. uint64_t is enougth for more that 2000000 years.
  */
uint64_t read_system_time(void)
{
  return(system_time);
}
/**
  * @brief  Just delay function that used system time.
  * @param  delay_time: Delay time in microSecond.
  * @retval None
  */
void sys_delay(uint64_t delay_time/* milliSecond*/)
{
  volatile uint64_t temp;

  temp = read_system_time();
  temp = temp + delay_time;
  while(read_system_time()< temp ){};
}

/**
  * @brief  Just delay function that program circle.
  * @param  delay_time: Delay time in microSecond.
  * @retval None
  */
void delay( unsigned int val)
{
  for( ; val != 0; val--)
  {
    __NOP();
  }
}


/************** TIMER 2 CAPTURE ***************************/
/**
  * @brief  Init GPIO and TIMER 2 channel like input capture.
  * @param  ch_number: Specifies the TIMER3 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  TIM_TIxExternalCLKSource:
  *   This parameter can be one of the following values:
  *     @arg TIM_ICSelection_DirectTI  : TIM Input 1 is selected to be connected to IC1.
  *     @arg TIM_ICSelection_IndirectTI: TIM Input 1 is selected to be connected to IC2.
  *     @arg TIM_ICSelection_TRC       : TIM Input 1 is selected to be connected to TRC.
  * @param  TIM_ICPolarity:
  *     @arg TIM_ICPolarity_Rising
  *     @arg TIM_ICPolarity_Falling
  * @param  TIM_ICPSC_divider:
  *     @arg TIM_ICPSC_DIV1
  *     @arg TIM_ICPSC_DIV2
  *     @arg TIM_ICPSC_DIV4
  *     @arg TIM_ICPSC_DIV8
  * @param  ICFilter:
  *     @arg 0-15
  * @retval None
  */
void TIMER2_CAPTURE_channel_init(
                                  uint16_t ch_number,
                                  uint16_t TIM_TIxExternalCLKSource,
                                  uint16_t TIM_ICPolarity,
                                  uint16_t TIM_ICPSC_divider,
                                  uint16_t ICFilter
                                )
{
  GPIO_InitTypeDef   GPIO_InitStructure;
  NVIC_InitTypeDef   NVIC_InitStructure;
  TIM_ICInitTypeDef  TIM_ICInitStructure;

  /* GPIO  */
  /* GPIOA clock enable (for timer3) */
  /* Connect clock to port A for ch 1(PA0),ch2(PA1),ch3 (PA2),ch4 (PA3). */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

  if(ch_number==1)
  {
    /* TIM2 channel 1 pin (PA.00) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_0;
  }
  else if(ch_number==2)
  {
    /* TIM2 channel 2 pin (PA.01) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_1;
  }
  else if(ch_number==3)
  {
    /* TIM2 channel 3 pin (PB.02) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_2;
  }
  else if(ch_number==4)
  {
    /* TIM2 channel 4 pin (PB.03) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_3;
  }
  else
  {
    while(1){};
  }
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IN_FLOATING;;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOA, &GPIO_InitStructure);

  /*   NVIC  */
  /* Enable the TIM2 global Interrupt */
  /* TBD priority */
  NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIMER2_CAPTURE_PRIORITY_GROUP;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIMER2_CAPTURE_SUB_PRIORITY;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /*  TIMER  */
  /* TIM2 clock enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);

  /* TIM2 configuration: Input Capture mode. */
  if(ch_number==1)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
  }
  else if(ch_number==2)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
  }
  else if(ch_number==3)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
  }
  else if(ch_number==4)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
  }
  TIM_ICInitStructure.TIM_ICPolarity  = TIM_ICPolarity;
  TIM_ICInitStructure.TIM_ICSelection = TIM_TIxExternalCLKSource;
  TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_divider;
  TIM_ICInitStructure.TIM_ICFilter    = ICFilter;
  TIM_ICInit(TIM2, &TIM_ICInitStructure);
  /* TIM enable counter */
  TIM_Cmd(TIM2, ENABLE);
  /* Enable the CCx Interrupt Request */
  if(ch_number==1)
  {
    TIM_ITConfig(TIM2, TIM_IT_CC1|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==2)
  {
    TIM_ITConfig(TIM2, TIM_IT_CC2|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==3)
  {
    TIM_ITConfig(TIM2, TIM_IT_CC3|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==4)
  {
    TIM_ITConfig(TIM2, TIM_IT_CC4|TIM_IT_Update, ENABLE);
  }
}

/************** TIMER 3 CAPTURE ***************************/
/**
  * @brief  Init GPIO and TIMER3 channel like input capture.
  * @param  ch_number: Specifies the TIMER3 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  TIM_TIxExternalCLKSource:
  *   This parameter can be one of the following values:
  *     @arg TIM_ICSelection_DirectTI  : TIM Input 1 is selected to be connected to IC1.
  *     @arg TIM_ICSelection_IndirectTI: TIM Input 1 is selected to be connected to IC2.
  *     @arg TIM_ICSelection_TRC       : TIM Input 1 is selected to be connected to TRC.
  * @param  TIM_ICPolarity:
  *     @arg TIM_ICPolarity_Rising
  *     @arg TIM_ICPolarity_Falling
  * @param  TIM_ICPSC_divider:
  *     @arg TIM_ICPSC_DIV1
  *     @arg TIM_ICPSC_DIV2
  *     @arg TIM_ICPSC_DIV4
  *     @arg TIM_ICPSC_DIV8
  * @param  ICFilter:
  *     @arg 0-15
  * @retval None
  */
void TIMER3_CAPTURE_channel_init(
                                  uint16_t ch_number,
                                  uint16_t TIM_TIxExternalCLKSource,
                                  uint16_t TIM_ICPolarity,
                                  uint16_t TIM_ICPSC_divider,
                                  uint16_t ICFilter
                                )
{
  GPIO_InitTypeDef   GPIO_InitStructure;
  NVIC_InitTypeDef   NVIC_InitStructure;
  TIM_ICInitTypeDef  TIM_ICInitStructure;

  /*  GPIO  */
  /* GPIOA clock enable (for timer3) */
  /* Connect clock to port A for ch 1(PA6) and ch2(PA7). */
  GPIO_InitStructure.GPIO_Mode        = GPIO_Mode_IN_FLOATING;;
  GPIO_InitStructure.GPIO_Speed       = GPIO_Speed_50MHz;
  if((ch_number==1)||(ch_number==2))
  {
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    if(ch_number==1)
    {
      /* TIM3 channel 1 pin (PA.06) configuration */
      GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_6;
    }
    else if(ch_number==2)
    {
      /* TIM3 channel 2 pin (PA.07) configuration */
      GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_7;
    }
    GPIO_Init(GPIOA, &GPIO_InitStructure);
  }
  else if((ch_number==3)||(ch_number==4))
  {
    /* Connect clock to port B for ch 3(PB0) and ch4(PB1). */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    if(ch_number==3)
    {
      /* TIM3 channel 3 pin (PB.00) configuration */
      GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_0;
    }
    else if(ch_number==4)
    {
      /* TIM3 channel 4 pin (PB.01) configuration */
      GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_1;
    }
    GPIO_Init(GPIOB, &GPIO_InitStructure);
  }
  else
  {
    while(1){};
  }

  /*   NVIC  */
  /* Enable the TIM3 global Interrupt */
  /* TBD */
  NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIMER3_CAPTURE_PRIORITY_GROUP;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIMER3_CAPTURE_SUB_PRIORITY;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /*  TIMER  */
  /* TIM3 clock enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

  /* TIM3 configuration: Input Capture mode. */
  if(ch_number==1)
  {
    TIM_ICInitStructure.TIM_Channel   = TIM_Channel_1;
  }
  else if(ch_number==2)
  {
    TIM_ICInitStructure.TIM_Channel   = TIM_Channel_2;
  }
  else if(ch_number==3)
  {
    TIM_ICInitStructure.TIM_Channel   = TIM_Channel_3;
  }
  else if(ch_number==4)
  {
    TIM_ICInitStructure.TIM_Channel   = TIM_Channel_4;
  }
  TIM_ICInitStructure.TIM_ICPolarity  = TIM_ICPolarity;
  TIM_ICInitStructure.TIM_ICSelection = TIM_TIxExternalCLKSource;
  TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_divider;
  TIM_ICInitStructure.TIM_ICFilter    = ICFilter;
  TIM_ICInit(TIM3, &TIM_ICInitStructure);
  /* TIM enable counter */
  TIM_Cmd(TIM3, ENABLE);
  /* Enable the CCx Interrupt Request */
  if(ch_number==1)
  {
    TIM_ITConfig(TIM3, TIM_IT_CC1|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==2)
  {
    TIM_ITConfig(TIM3, TIM_IT_CC2|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==3)
  {
    TIM_ITConfig(TIM3, TIM_IT_CC3|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==4)
  {
    TIM_ITConfig(TIM3, TIM_IT_CC4|TIM_IT_Update, ENABLE);
  }
}

/************** TIMER 4 CAPTURE ***************************/
/**
  * @brief  Init GPIO and TIMER 4 channel like input capture.
  * @param  ch_number: Specifies the TIMER3 channel
  *     @arg 1
  *     @arg 2
  *     @arg 3
  *     @arg 4
  * @param  TIM_TIxExternalCLKSource:
  *   This parameter can be one of the following values:
  *     @arg TIM_ICSelection_DirectTI  : TIM Input 1 is selected to be connected to IC1.
  *     @arg TIM_ICSelection_IndirectTI: TIM Input 1 is selected to be connected to IC2.
  *     @arg TIM_ICSelection_TRC       : TIM Input 1 is selected to be connected to TRC.
  * @param  TIM_ICPolarity:
  *     @arg TIM_ICPolarity_Rising
  *     @arg TIM_ICPolarity_Falling
  * @param  TIM_ICPSC_divider:
  *     @arg TIM_ICPSC_DIV1
  *     @arg TIM_ICPSC_DIV2
  *     @arg TIM_ICPSC_DIV4
  *     @arg TIM_ICPSC_DIV8
  * @param  ICFilter:
  *     @arg 0-15
  * @retval None
  */
void TIMER4_CAPTURE_channel_init(
                                  uint16_t ch_number,
                                  uint16_t TIM_TIxExternalCLKSource,
                                  uint16_t TIM_ICPolarity,
                                  uint16_t TIM_ICPSC_divider,
                                  uint16_t ICFilter
                                )
{
  GPIO_InitTypeDef   GPIO_InitStructure;
  NVIC_InitTypeDef   NVIC_InitStructure;
  TIM_ICInitTypeDef  TIM_ICInitStructure;

  /*  GPIO  */
  /* GPIOB clock enable (for timer3) */
  /* Connect clock to port B for ch1 (PB6),ch2 (PB7),ch3 (PB8), ch4(PB9). */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

  if(ch_number==1)
  {
    /* TIM4 channel 1 pin (PB.06) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_6;
  }
  else if(ch_number==2)
  {
    /* TIM4 channel 2 pin (PB.07) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_7;
  }
  else if(ch_number==3)
  {
    /* TIM4 channel 3 pin (PB.08) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_8;
  }
  else if(ch_number==4)
  {
    /* TIM4 channel 4 pin (PB.09) configuration */
    GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_9;
  }
  else
  {
    while(1){};
  }
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IN_FLOATING;;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  GPIO_Init(GPIOB, &GPIO_InitStructure);

  /*   NVIC  */
  /* Enable the TIM4 global Interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIMER4_CAPTURE_PRIORITY_GROUP;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIMER4_CAPTURE_SUB_PRIORITY;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /*  TIMER  */
  /* TIM4 clock enable */
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);

  /* TIM4 configuration: Input Capture mode. */
  if(ch_number==1)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
  }
  else if(ch_number==2)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
  }
  else if(ch_number==3)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
  }
  else if(ch_number==4)
  {
    TIM_ICInitStructure.TIM_Channel = TIM_Channel_4;
  }
  TIM_ICInitStructure.TIM_ICPolarity  = TIM_ICPolarity;
  TIM_ICInitStructure.TIM_ICSelection = TIM_TIxExternalCLKSource;
  TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_divider;
  TIM_ICInitStructure.TIM_ICFilter    = ICFilter;
  TIM_ICInit(TIM4, &TIM_ICInitStructure);
  /* TIM enable counter */
  TIM_Cmd(TIM4, ENABLE);
  /* Enable the CCx Interrupt Request */
  if(ch_number==1)
  {
    TIM_ITConfig(TIM4, TIM_IT_CC1|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==2)
  {
    TIM_ITConfig(TIM4, TIM_IT_CC2|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==3)
  {
    TIM_ITConfig(TIM4, TIM_IT_CC3|TIM_IT_Update, ENABLE);
  }
  else if(ch_number==4)
  {
    TIM_ITConfig(TIM4, TIM_IT_CC4|TIM_IT_Update, ENABLE);
  }
}




/************** TIMER 1 PERIODICAL INTERUPT ***************************/
/**
  * @brief  Init TIMER 1 like periodical interupt source.
  * @param  interupt perion: Specify period of interupt
  * @retval None
  */
void TIMER1_period_interupt_init(uint16_t period_of_interupt)
{
  NVIC_InitTypeDef   NVIC_InitStructure;
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;

  /*   NVIC  */
  /* Enable the TIM1 global Interrupt */
  NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn;
  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = TIMER1_PERIOD_INTERUPT_PRIORITY_GROUP;
  NVIC_InitStructure.NVIC_IRQChannelSubPriority = TIMER1_PERIOD_INTERUPT_SUB_PRIORITY_GROUP;
  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  NVIC_Init(&NVIC_InitStructure);

  /* Init Timer1 like PWM. */
  RCC_APB2PeriphClockCmd (RCC_APB2Periph_TIM1, ENABLE);
  // Time Base configuration
  TIM_TimeBaseStructure.TIM_Period        = period_of_interupt;
  TIM_TimeBaseStructure.TIM_Prescaler     = 72;         // Ftimer=fsys/Prescaler,for Prescaler=72 ,Ftimer=1MHz
  TIM_TimeBaseStructure.TIM_ClockDivision = 0;
  TIM_TimeBaseStructure.TIM_CounterMode   = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

  TIM_ClearITPendingBit(TIM1, TIM_IT_Update);   // Clear Interrupt
  TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
  //TIM_Cmd(TIM1, ENABLE);
//  NVIC_EnableIRQ(TIM1_UP_IRQn);
}

/**
  * @brief  This function handles TIM2 global interrupt request.
  * @param  None
  * @retval None
TIM1_UP_IRQn
  */
void TIM1_UP_IRQHandler(void)
{
  if(TIM_GetITStatus(TIM1, TIM_IT_Update) == SET)
  {
    TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
    //GPIO_SetBits( GPIOB, GPIO_Pin_1);
 //   GPIO_SetBits( GPIOB, GPIO_Pin_0);


    /* Start main processing function. */
    // We are temporary disable service process
    /////////////    start_processing(local_board,local_gyro);
    //This read data from sensors to local_board structure.
    start_processing_sensors(local_board,local_gyro);
    //GPIO_ResetBits( GPIOB, GPIO_Pin_1);
 //   GPIO_ResetBits( GPIOB, GPIO_Pin_0);

  }

}
// This function start timer that will be generate interrupt for updating
// all sensors states. Input parameter for function is pointer to
// sensors states structure called "board" and pointer to gyro object( here we
// describe access to gyro througth pointer to gyro function).
void start_main_interupt_function(control_pid_est_t *board,sensor_t *gyro)
{
//  local_board = board;
//  local_gyro  = gyro;
//  TIM_Cmd(TIM1, ENABLE);
//  NVIC_EnableIRQ(TIM1_UP_IRQn);
}

void start_main_interupt_function_sensor(sensor_raw_data_t *board,sensor_t *gyro)
{
  local_board = board;
  local_gyro  = gyro;
  TIM_Cmd(TIM1, ENABLE);
  NVIC_EnableIRQ(TIM1_UP_IRQn);
}




